#include "Quaternion.h"
#include "Quaternion.h"
#include <math.h>


Quaternion::Quaternion()
{
	w = 1.0;
	x = y = z = 0.0;
}


Quaternion::~Quaternion()
{
}
Quaternion Quaternion::Copy()
{
	Quaternion Q;
	Q.w = w;
	Q.x = x;
	Q.y = y;
	Q.z = z;
	return Q;
}
Quaternion Quaternion::operator*(Quaternion other)
{
	double Ax = x;
	double Ay = y;
	double Az = z;
	double Aw = w;
	double Bx = other.x;
	double By = other.y;
	double Bz = other.z;
	double Bw = other.w;
	Quaternion	Q;
	Q.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx;
	Q.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By;
	Q.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz;
	Q.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw;
	return Q;

}
Vector3 Quaternion::mul(Quaternion q, Vector3 other)
{
	double w = q.w;
	double 	x = q.x;
	double 	y = q.y;
	double z = q.z;
	double Vx = other.x;
	double Vy = other.y;
	double Vz = other.z;
	double ww = w * w;
	double w2 = w * 2;
	double wx2 = w2 * x;
	double wy2 = w2 * y;
	double wz2 = w2 * z;
	double 	xx = x * x;
	double 	x2 = x * 2;
	double xy2 = x2 * y;
	double xz2 = x2 * z;
	double yy = y * y;
	double 	yz2 = 2 * y * z;
	double 	zz = z * z;
	return Vector3(ww * Vx + wy2 * Vz - wz2 * Vy + xx * Vx + xy2 * Vy + xz2 * Vz - zz * Vx - yy * Vx,
		xy2 * Vx + yy * Vy + yz2 * Vz + wz2 * Vx - zz * Vy + ww * Vy - wx2 * Vz - xx * Vy,
		xz2 * Vx + yz2 * Vy + zz * Vz - wy2 * Vx - yy * Vz + wx2 * Vy - xx * Vz + ww * Vz);

}
Vector3 Quaternion::operator*(Vector3 other)
{
	double w = w;
	double 	x = x;
	double 	y = y;
	double z = z;
	double Vx = other.x;
	double Vy = other.y;
	double Vz = other.z;
	double ww = w * w;
	double w2 = w * 2;
	double wx2 = w2 * x;
	double wy2 = w2 * y;
	double wz2 = w2 * z;
	double 	xx = x * x;
	double 	x2 = x * 2;
	double 	xy2 = x2 * y;
	double xz2 = x2 * z;
	double yy = y * y;
	double 	yz2 = 2 * y * z;
	double 	zz = z * z;
	return Vector3(ww * Vx + wy2 * Vz - wz2 * Vy + xx * Vx + xy2 * Vy + xz2 * Vz - zz * Vx - yy * Vx,
		xy2 * Vx + yy * Vy + yz2 * Vz + wz2 * Vx - zz * Vy + ww * Vy - wx2 * Vz - xx * Vy,
		xz2 * Vx + yz2 * Vy + zz * Vz - wy2 * Vx - yy * Vz + wx2 * Vy - xx * Vz + ww * Vz);

}

Quaternion Quaternion::new_rotate_euler(double heading, double attitude, double bank)
{
	double c1 = cos(heading / 2.0);
	double s1 = sin(heading / 2.0);
	double c2 = cos(attitude / 2.0);
	double s2 = sin(attitude / 2.0);
	double c3 = cos(bank / 2.0);
	double s3 = sin(bank / 2.0);

	Quaternion Q;
	w = Q.w = c1 * c2 * c3 - s1 * s2 * s3;
	x = Q.x = s1 * s2 * c3 + c1 * c2 * s3;
	y = Q.y = s1 * c2 * c3 + c1 * s2 * s3;
	z=Q.z = c1 * s2 * c3 - s1 * c2 * s3;
	return Q;


}